#include "motor.h"
#include "core_cmInstr.h"

void	Init_StepMotor(void)
{
	GPIO_InitType GPIO_InitStructure;

	GPIO_InitStruct(&GPIO_InitStructure);

	GPIO_InitStructure.Pin = FPF_IN1_PIN | FPF_IN2_PIN | FPF_IN3_PIN | FPF_IN4_PIN | PG_IN1_PIN | PG_IN2_PIN | PG_IN4_PIN;
	GPIO_InitStructure.GPIO_Mode = GPIO_MODE_OUTPUT_PP;
	GPIO_InitPeripheral(FPF_IN_PORT, &GPIO_InitStructure);

	GPIO_InitStructure.Pin = PG_IN3_PIN;
	GPIO_InitPeripheral(PG_IN3_PORT, &GPIO_InitStructure);

	FPFMotorPowerOff();
	PGMotorPowerOff();
	PGMotor.DirLast = PGMotor.Dir = MOTOR_CW;
	FPFMotor.DirLast = FPFMotor.Dir = MOTOR_CW;	
}

void	NopFunc(void)
{
}

void	InitStepMotorTimer(void)
{
	TIM_TimeBaseInitType TIM_TimeBaseStructure;
	uint16_t PrescalerValue = 0;
  NVIC_InitType NVIC_InitStructure;

	FPFMotor.IntFunc = PGMotor.IntFunc = NopFunc;
    /* Compute the prescaler value */
  RCC_EnableAPB1PeriphClk(RCC_APB1_PERIPH_TIM3, ENABLE);
  RCC_EnableAPB2PeriphClk(RCC_APB2_PERIPH_TIM1, ENABLE);
  RCC_ConfigPclk1(RCC_HCLK_DIV1);
  RCC_ConfigPclk2(RCC_HCLK_DIV1);

	/* Enable the TIM1 global Interrupt */
	NVIC_InitStructure.NVIC_IRQChannel                   = PGTimerIRQ;
	NVIC_InitStructure.NVIC_IRQChannelPriority           = 1;
	NVIC_InitStructure.NVIC_IRQChannelCmd                = ENABLE;
	NVIC_Init(&NVIC_InitStructure);
	NVIC_InitStructure.NVIC_IRQChannel                   = FPFTimerIRQ;
	NVIC_Init(&NVIC_InitStructure);

	PrescalerValue = (uint16_t)(SystemCoreClock / STEP_MOTOR_SYS_CLOCK) - 1;
	/* Time base configuration */
	TIM_TimeBaseStructure.Period    = STEP_MOTOR_SYS_CLOCK / 1000;
	TIM_TimeBaseStructure.Prescaler = PrescalerValue;
	TIM_TimeBaseStructure.ClkDiv    = 0;
	TIM_TimeBaseStructure.CntMode   = TIM_CNT_MODE_UP;

 	TIM_InitTimeBase(FPFTimer, &TIM_TimeBaseStructure);
  TIM_ConfigPrescaler(FPFTimer, PrescalerValue, TIM_PSC_RELOAD_MODE_IMMEDIATE);
   /* TIM3 enable update irq */
	 
	TIM_ClrIntPendingBit(FPFTimer, TIM_INT_UPDATE);
  TIM_ConfigInt(FPFTimer, TIM_INT_UPDATE, ENABLE);
  /* TIM3 enable counter */
//  TIM_Enable(FPFTimer, ENABLE);
	
  TIM_InitTimeBase(PGTimer, &TIM_TimeBaseStructure);
	/* Prescaler configuration */
	TIM_ConfigPrescaler(PGTimer, PrescalerValue, TIM_PSC_RELOAD_MODE_IMMEDIATE);

	/* TIM1 enable update irq */
  TIM_ClrIntPendingBit(PGTimer, TIM_INT_UPDATE);
	TIM_ConfigInt(PGTimer, TIM_INT_UPDATE, ENABLE);

	/* TIM1 enable counter */
	TIM_Enable(PGTimer, ENABLE);
}

void	FPFMotorPhaseCtrl(void)
{
	switch(FPFMotor.phase)
	{
		case	0:
			GPIO_SetBits(FPF_IN_PORT,FPF_IN1_PIN);
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN2_PIN);
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN3_PIN);
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN4_PIN);
			break;
		case	1:
			GPIO_SetBits(FPF_IN_PORT,FPF_IN1_PIN);
			GPIO_SetBits(FPF_IN_PORT,FPF_IN2_PIN);
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN3_PIN);
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN4_PIN);
			break;
		case	2:
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN1_PIN);
			GPIO_SetBits(FPF_IN_PORT,FPF_IN2_PIN);
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN3_PIN);
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN4_PIN);
			break;
		case	3:
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN1_PIN);
			GPIO_SetBits(FPF_IN_PORT,FPF_IN2_PIN);
			GPIO_SetBits(FPF_IN_PORT,FPF_IN3_PIN);
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN4_PIN);
			break;
		case	4:
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN1_PIN);
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN2_PIN);
			GPIO_SetBits(FPF_IN_PORT,FPF_IN3_PIN);
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN4_PIN);
			break;
		case	5:
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN1_PIN);
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN2_PIN);
			GPIO_SetBits(FPF_IN_PORT,FPF_IN3_PIN);
			GPIO_SetBits(FPF_IN_PORT,FPF_IN4_PIN);
			break;
		case	6:
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN1_PIN);
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN2_PIN);
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN3_PIN);
			GPIO_SetBits(FPF_IN_PORT,FPF_IN4_PIN);
			break;
		default:
			GPIO_SetBits(FPF_IN_PORT,FPF_IN1_PIN);
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN2_PIN);
			GPIO_ResetBits(FPF_IN_PORT,FPF_IN3_PIN);
			GPIO_SetBits(FPF_IN_PORT,FPF_IN4_PIN);
			break;
	}
	if (FPFMotor.Dir == MOTOR_CW)
	{
		if (FPFMotor.phase >= MOTOR_PHASE_MAX - 1)
			FPFMotor.phase = 0;
		else
			FPFMotor.phase ++;			
	}
	else
	{
		if (FPFMotor.phase == 0)
			FPFMotor.phase = MOTOR_PHASE_MAX - 1;
		else
			FPFMotor.phase --;
	}
}

void	FPFMotorPowerOn()
{
	if (FPFMotor.Dir == MOTOR_CW)
		FPFMotor.phase = 0;
	else
		FPFMotor.phase = MOTOR_PHASE_MAX - 1;
	FPFMotorPhaseCtrl();
}

void	FPFMotorPowerOff(void)
{
	GPIO_ResetBits(FPF_IN_PORT,FPF_IN1_PIN);
	GPIO_ResetBits(FPF_IN_PORT,FPF_IN2_PIN);
	GPIO_ResetBits(FPF_IN_PORT,FPF_IN3_PIN);
	GPIO_ResetBits(FPF_IN_PORT,FPF_IN4_PIN);
}

void	FPFMotorTimerSet(U16 time)
{
	FPFTimer->AR = time >> 1;
}

void	FPFTimerStop(void)
{
	TIM_Enable(FPFTimer, DISABLE);
  FPFTimer->STS &= (uint32_t)~TIM_INT_UPDATE;
}

void	FPFMotorTimerStart(U16 time)
{
	FPFTimer->AR = time >> 1;
  FPFTimer->STS &= (uint32_t)~TIM_INT_UPDATE;
	TIM_Enable(FPFTimer, ENABLE);
}

void	FPFMotorStop(void)
{
	FPFTimerStop();
	FPFMotorPowerOff();
	FPFMotor.Busy = false;
	FPFMotor.DirLast = FPFMotor.Dir;
}

void	FPFMotorDrive(void)
{
	FPFMotorPhaseCtrl();
	FPFMotor.Count --;
}

void	FPFMotorBrake(void)
{
	FPFMotorDrive();
	if (--FPFMotor.BrakeDist == 0)
	{
		FPFMotor.IntFunc = FPFMotorStop;
		FPFMotorTimerSet(MOTORPOWERON);
	}
	else
	{
		FPFMotorTimerSet(*FPFMotor.BrakeTable ++);
	}
}

void	FPFMotorMove(void)
{
	FPFMotorDrive();
	if (FPFMotor.Count == 0)
	{
		FPFMotor.IntFunc = FPFMotorStop;
		FPFMotorTimerSet(MOTORPOWERON);
	}
	else if (FPFMotor.BrakeDist == FPFMotor.Count)
	{
		FPFMotor.IntFunc = FPFMotorBrake;
	}
}

void	FPFMotorAccel(void)
{
	FPFMotorDrive();
	if ((--FPFMotor.AccelDist == 0) ||(FPFMotor.BrakeDist == FPFMotor.Count))
	{
		if (FPFMotor.BrakeDist == FPFMotor.Count)
			FPFMotor.IntFunc = FPFMotorBrake;
		else
			FPFMotor.IntFunc = FPFMotorMove;
	}
	FPFMotorTimerSet(*FPFMotor.AccelTable ++);
}

void	FPFMotorStart(void)
{
	U16 Accel,Brake,Brake1;

	FPFTimerStop();
	FPFMotor.AccelTable = (U16 *)FPFmAccelTbl;
	FPFMotor.BrakeTable = (U16 *)FPFmBrakeTbl;
	Accel = *FPFMotor.AccelTable++;
	Brake = *FPFMotor.BrakeTable++;
	if (FPFMotor.Count < Accel * 2)
		Accel = FPFMotor.Count >> 1;
	Brake1 = FPFMotor.Count - Accel;
	if( Brake1 < Brake )
	{
		FPFMotor.BrakeTable += Brake - Brake1;
		Brake = Brake1;
	}
	FPFMotor.AccelDist = Accel;
	FPFMotor.BrakeDist = Brake;
	FPFMotor.Busy = true;
	FPFMotor.IntFunc =  (Accel == 0)? FPFMotorBrake : FPFMotorAccel;
	FPFMotorPowerOn();
	FPFMotorTimerStart(MOTORPOWERON);
	return;
}

void	FPFMotorSyncro(void)
{
	while(true)
	{
		if(!FPFMotor.Busy)
			break;
	}
}

void	FPFMotor_Run(U16 count,U8 motion)
{
	if (count != 0)
	{
		FPFMotorSyncro();
		FPFMotor.Count = count;
		FPFMotor.Motion = motion;
		switch(motion)
		{
			case	FPF_MOTOR_MAXMOVE:
				FPFMotor.Dir = MOTOR_CW;
			break;
			case	FPF_MOTOR_XFMOVE:
				FPFMotor.Dir = !FPFMotor.DirLast;
				break;
			case	FPF_MOTOR_HOME:
			default:
				FPFMotor.Dir = MOTOR_CCW;
			break;
		}
		FPFMotorStart();
	}
}

void	PGMotorPhaseCtrl(void)
{
	switch(PGMotor.phase)
	{
		case	0:
			GPIO_SetBits(PG_IN1_PORT,PG_IN1_PIN);
			GPIO_ResetBits(PG_IN2_PORT,PG_IN2_PIN);
			GPIO_ResetBits(PG_IN3_PORT,PG_IN3_PIN);
			GPIO_ResetBits(PG_IN4_PORT,PG_IN4_PIN);
			break;
		case	1:
			GPIO_SetBits(PG_IN1_PORT,PG_IN1_PIN);
			GPIO_SetBits(PG_IN2_PORT,PG_IN2_PIN);
			GPIO_ResetBits(PG_IN3_PORT,PG_IN3_PIN);
			GPIO_ResetBits(PG_IN4_PORT,PG_IN4_PIN);
			break;
		case	2:
			GPIO_ResetBits(PG_IN1_PORT,PG_IN1_PIN);
			GPIO_SetBits(PG_IN2_PORT,PG_IN2_PIN);
			GPIO_ResetBits(PG_IN3_PORT,PG_IN3_PIN);
			GPIO_ResetBits(PG_IN4_PORT,PG_IN4_PIN);
			break;
		case	3:
			GPIO_ResetBits(PG_IN1_PORT,PG_IN1_PIN);
			GPIO_SetBits(PG_IN2_PORT,PG_IN2_PIN);
			GPIO_SetBits(PG_IN3_PORT,PG_IN3_PIN);
			GPIO_ResetBits(PG_IN4_PORT,PG_IN4_PIN);
			break;
		case	4:
			GPIO_ResetBits(PG_IN1_PORT,PG_IN1_PIN);
			GPIO_ResetBits(PG_IN2_PORT,PG_IN2_PIN);
			GPIO_SetBits(PG_IN3_PORT,PG_IN3_PIN);
			GPIO_ResetBits(PG_IN4_PORT,PG_IN4_PIN);
			break;
		case	5:
			GPIO_ResetBits(PG_IN1_PORT,PG_IN1_PIN);
			GPIO_ResetBits(PG_IN2_PORT,PG_IN2_PIN);
			GPIO_SetBits(PG_IN3_PORT,PG_IN3_PIN);
			GPIO_SetBits(PG_IN4_PORT,PG_IN4_PIN);
			break;
		case	6:
			GPIO_ResetBits(PG_IN1_PORT,PG_IN1_PIN);
			GPIO_ResetBits(PG_IN2_PORT,PG_IN2_PIN);
			GPIO_ResetBits(PG_IN3_PORT,PG_IN3_PIN);
			GPIO_SetBits(PG_IN4_PORT,PG_IN4_PIN);
			break;
		default:
			GPIO_SetBits(PG_IN1_PORT,PG_IN1_PIN);
			GPIO_ResetBits(PG_IN2_PORT,PG_IN2_PIN);
			GPIO_ResetBits(PG_IN3_PORT,PG_IN3_PIN);
			GPIO_SetBits(PG_IN4_PORT,PG_IN4_PIN);
			break;
	}
	if (PGMotor.Dir == MOTOR_CW)
	{
		if (PGMotor.phase >= MOTOR_PHASE_MAX - 1)
			PGMotor.phase = 0;
		else
			PGMotor.phase ++;			
	}
	else
	{
		if (PGMotor.phase == 0)
			PGMotor.phase = MOTOR_PHASE_MAX - 1;
		else
			PGMotor.phase --;
	}
}

void	PGMotorPowerOn()
{
	if (PGMotor.Dir == MOTOR_CW)
		PGMotor.phase = 0;
	else
		PGMotor.phase = MOTOR_PHASE_MAX - 1;
	PGMotorPhaseCtrl();
}

void	PGMotorPowerOff(void)
{
	GPIO_ResetBits(PG_IN1_PORT,PG_IN1_PIN);
	GPIO_ResetBits(PG_IN2_PORT,PG_IN2_PIN);
	GPIO_ResetBits(PG_IN3_PORT,PG_IN3_PIN);
	GPIO_ResetBits(PG_IN4_PORT,PG_IN4_PIN);
}

void	PGMotorTimerSet(U16 time)
{
	PGTimer->AR = time;
}

void	PGTimerStop(void)
{
	TIM_Enable(PGTimer, DISABLE);
  PGTimer->STS &= (uint32_t)~TIM_INT_UPDATE;
}

void	PGMotorTimerStart(U16 time)
{
	PGTimer->AR = time;
  PGTimer->STS &= (uint32_t)~TIM_INT_UPDATE;
	TIM_Enable(PGTimer, ENABLE);
}

void	PGMotorStop(void)
{
	PGTimerStop();
	PGMotorPowerOff();
	PGMotor.Busy = false;
	PGMotor.DirLast = PGMotor.Dir;
}

void	PGMotorDrive(void)
{
	PGMotorPhaseCtrl();
	PGMotor.Count --;
}

void	PGMotorBrake(void)
{
	PGMotorDrive();
	if (--PGMotor.BrakeDist == 0)
	{
		PGMotor.IntFunc = PGMotorStop;
		PGMotorTimerSet(MOTORPOWERON);
	}
	else
	{
		PGMotorTimerSet(*PGMotor.BrakeTable ++);
	}
}

void	PGMotorMove(void)
{
	PGMotorDrive();
	if (PGMotor.Count == 0)
	{
		PGMotor.IntFunc = PGMotorStop;
		PGMotorTimerSet(MOTORPOWERON);
	}
	else if (PGMotor.BrakeDist == PGMotor.Count)
	{
		PGMotor.IntFunc = PGMotorBrake;
	}
}

void	PGMotorAccel(void)
{
	PGMotorDrive();
	if ((--PGMotor.AccelDist == 0) ||(PGMotor.BrakeDist == PGMotor.Count))
	{
		if (PGMotor.BrakeDist == PGMotor.Count)
			PGMotor.IntFunc = PGMotorBrake;
		else
			PGMotor.IntFunc = PGMotorMove;
	}
	PGMotorTimerSet(*PGMotor.AccelTable ++);
}

void	PGMotorStart(void)
{
	U16 Accel,Brake,Brake1;

	PGTimerStop();
	PGMotor.AccelTable = (U16 *)FPFmAccelTbl;
	PGMotor.BrakeTable = (U16 *)FPFmBrakeTbl;
	Accel = *PGMotor.AccelTable++;
	Brake = *PGMotor.BrakeTable++;
	if (PGMotor.Count < Accel * 2)
		Accel = PGMotor.Count >> 1;
	Brake1 = PGMotor.Count - Accel;
	if( Brake1 < Brake )
	{
		PGMotor.BrakeTable += Brake - Brake1;
		Brake = Brake1;
	}
	PGMotor.AccelDist = Accel;
	PGMotor.BrakeDist = Brake;
	PGMotor.Busy = true;
	PGMotor.IntFunc =  (Accel == 0)? PGMotorBrake : PGMotorAccel;
	PGMotorPowerOn();
	PGMotorTimerStart(MOTORPOWERON);
}

void	PGMotorSyncro(void)
{
	while(true)
	{
		if(!PGMotor.Busy)
			break;
	}
}

void	PGMotor_Run(U16 count,U8 motion)
{
	if (count != 0)
	{
		PGMotorSyncro();
		PGMotor.Count = count;
		PGMotor.Motion = motion;
		switch(motion)
		{
			case	FPF_MOTOR_MAXMOVE:
				PGMotor.Dir = MOTOR_CW;
			break;
			case	FPF_MOTOR_XFMOVE:
				PGMotor.Dir = !PGMotor.DirLast;
				break;
			case	FPF_MOTOR_HOME:
			default:
				PGMotor.Dir = MOTOR_CCW;
			break;
		}
		PGMotorStart();
	}
}

void FPFTimerIRQHandler(void)
{
    if (TIM_GetIntStatus(FPFTimer, TIM_INT_UPDATE) != RESET)
    {
			TIM_ClrIntPendingBit(FPFTimer, TIM_INT_UPDATE);
			FPFMotor.IntFunc();
    }
}

void PGTimerIRQHandler(void)
{
    if (TIM_GetIntStatus(PGTimer, TIM_INT_UPDATE) != RESET)
    {
      TIM_ClrIntPendingBit(PGTimer, TIM_INT_UPDATE);
			PGMotor.IntFunc();
   }
}

void	Init_UP_DOWN_DCMotor(void)
{
	GPIO_InitType GPIO_InitStructure;

	GPIO_InitStruct(&GPIO_InitStructure);

	GPIO_InitStructure.Pin = UP_IN1_PIN | UP_IN2_PIN | DOWN_IN1_PIN;
	GPIO_InitStructure.GPIO_Mode = GPIO_MODE_OUTPUT_PP;
	GPIO_InitPeripheral(UP_IN1_PORT, &GPIO_InitStructure);
//	GPIO_InitStructure.Pin = SPK_PIN;
//	GPIO_InitPeripheral(SPK_PORT, &GPIO_InitStructure);
	GPIO_InitStructure.Pin = DOWN_IN2_PIN;
	GPIO_InitPeripheral(DOWN_IN2_PORT, &GPIO_InitStructure);
	GPIO_ResetBits(UP_IN1_PORT,UP_IN1_PIN);
	GPIO_ResetBits(UP_IN2_PORT,UP_IN2_PIN);
	GPIO_ResetBits(DOWN_IN1_PORT,DOWN_IN1_PIN);
	GPIO_ResetBits(DOWN_IN2_PORT,DOWN_IN2_PIN);
}

void	RUN_UP_DOWN_DCMotor(u8 motor)
{
	swDelay = 50;
	while(true)
	{
		if (motor == 0)
		{
			GPIO_SetBits(UP_IN1_PORT,UP_IN1_PIN);
			GPIO_ResetBits(UP_IN2_PORT,UP_IN2_PIN);
		}
		else
		{
			GPIO_SetBits(DOWN_IN1_PORT,DOWN_IN1_PIN);
			GPIO_ResetBits(DOWN_IN2_PORT,DOWN_IN2_PIN);
		}
		if (swDelay == 0)
			break;
	}
	swDelay = 5;
	while(true)
	{
		if (motor == 0)
		{
			GPIO_SetBits(UP_IN1_PORT,UP_IN1_PIN);
			GPIO_SetBits(UP_IN2_PORT,UP_IN2_PIN);
		}
		else
		{
			GPIO_SetBits(DOWN_IN1_PORT,DOWN_IN1_PIN);
			GPIO_SetBits(DOWN_IN2_PORT,DOWN_IN2_PIN);
		}
		if (swDelay == 0)
			break;
	}
	if (motor == 0)
	{
		GPIO_ResetBits(UP_IN1_PORT,UP_IN1_PIN);
		GPIO_ResetBits(UP_IN2_PORT,UP_IN2_PIN);
	}
	else
	{
		GPIO_ResetBits(DOWN_IN1_PORT,DOWN_IN1_PIN);
		GPIO_ResetBits(DOWN_IN2_PORT,DOWN_IN2_PIN);
	}
	return;
}

